/*
 * Vec3D.cpp
 *
 *  Created on: Jun 24, 2012
 *      Author: burek
 */

#include "Vec3D.h"
#include <cmath>

const Vec3D Vec3D::Zero(0.0f, 0.0f, 0.0f);
const Vec3D Vec3D::One(1.0f, 1.0f, 1.0f);

const Vec3D Vec3D::XUnit(1.0f, 0.0f, 0.0f);
const Vec3D Vec3D::YUnit(0.0f, 1.0f, 0.0f);
const Vec3D Vec3D::ZUnit(0.0f, 0.0f, 1.0f);

const Vec3D Vec3D::NegXUnit(-1.0f, 0.0f, 0.0f);
const Vec3D Vec3D::NegYUnit(0.0f, -1.0f, 0.0f);
const Vec3D Vec3D::NegZUnit(0.0f, 0.0f, -1.0f);

Vec3D::Vec3D( void ) { X = 0.0f; Y = 0.0f; Z = 0.0f; }
Vec3D::Vec3D( const float x, const float y, const float z )
{ X = x; Y = y; Z = z; }

Vec3D& Vec3D::operator= ( const Vec3D& A )
{ X = A.X; Y = A.Y; Z = A.Z; return *this; }

bool Vec3D::operator== ( const Vec3D& A ) const
{ return X == A.X && Y == A.Y && Z == A.Z; }

bool Vec3D::operator!= ( const Vec3D& A ) const
{ return X != A.X || Y != A.Y || Z != A.Z; }

Vec3D Vec3D::operator+ ( const Vec3D& A ) const
{ return Vec3D( X + A.X, Y + A.Y, Z + A.Z ); }

Vec3D Vec3D::operator- ( const Vec3D& A ) const
{ return Vec3D( X - A.X, Y - A.Y, Z - A.Z ); }

Vec3D Vec3D::operator* ( const Vec3D& A ) const
{ return Vec3D( X * A.X, Y * A.Y, Z * A.Z ); }

Vec3D Vec3D::operator/ ( const Vec3D& A ) const
{ return Vec3D( X / A.X, Y / A.Y, Z / A.Z ); }

Vec3D Vec3D::operator* ( const float A ) const
{ return Vec3D( X * A, Y * A, Z * A ); }

Vec3D Vec3D::operator/ ( const float A ) const
{ return Vec3D( X / A, Y / A, Z / A ); }

float Vec3D::dot( const Vec3D& A ) const
{ return A.X*X + A.Y*Y + A.Z*Z; }

float Vec3D::length( void ) const
{ return sqrt(X * X + Y * Y + Z * Z); }

float Vec3D::lengthSquared( void ) const
{ return (X * X + Y * Y + Z * Z); }

void Vec3D::set(float x, float y, float z)
{
	X = x;
	Y = y;
	Z = z;
}

Vec3D Vec3D::cross(const Vec3D & vec) const
{
  return Vec3D(Y * vec.Z - Z * vec.Y,
			   Z * vec.X - X * vec.Z,
			   X * vec.Y - Y * vec.X);
}

Vec3D Vec3D::normalize(void) const
{
	float len = length();
	if(len != 0.0f)
		return Vec3D(X / len, Y / len, Z / len);
	else
		return *this;
}

float Vec3D::distanceTo(const Vec3D& A) const
{
	float dX = A.X - X, dY = A.Y - Y, dZ = A.Z - Z;
	return sqrt(dX * dX + dY * dY + dZ * dZ);
}

float Vec3D::distanceSquared(const Vec3D& A) const
{
	float dX = A.X - X, dY = A.Y - Y, dZ = A.Z - Z;
	return (dX * dX + dY * dY + dZ * dZ);
}

void Vec3D::normalizeIP(void)
{
	float len = length();
	X = X / len; Y = Y / len; Z = Z / len;
}

void Vec3D::operator += ( const Vec3D& A )
{ X += A.X; Y += A.Y; Z += A.Z; }

void Vec3D::operator *= ( const Vec3D& A )
{ X *= A.X; Y *= A.Y; Z *= A.Z; }

void Vec3D::operator -= ( const Vec3D& A )
{ X -= A.X; Y -= A.Y; Z -= A.Z; }

void Vec3D::operator *= ( const float A )
{ X *= A; Y *= A; Z *= A; }

void Vec3D::operator /= ( const float A )
{ X /= A; Y /= A; Z /= A; }

void Vec3D::transform(const float matrix[])
{
	  float vx = X * matrix[0+0] +
				 Y * matrix[4+0] +
				 Z * matrix[8+0] +
					 matrix[12+0];

	  float vy = X * matrix[0+1] +
				 Y * matrix[4+1] +
				 Z * matrix[8+1] +
					 matrix[12+1];

	  float vz = X * matrix[0+2] +
				 Y * matrix[4+2] +
				 Z * matrix[8+2] +
					 matrix[12+2];

	  X = vx;
	  Y = vy;
	  Z = vz;
}

void Vec3D::lerpIP( const Vec3D& A, const float x )
{ X += (A.X - X) * x; Y += (A.Y - Y) * x; Z += (A.Z - Z) * x; }

Vec3D Vec3D::lerp( const Vec3D& A, const float x ) const
{ return Vec3D(X + (A.X - X) * x, Y + (A.Y - Y) * x, Z + (A.Z - Z) * x); }

Vec3D Vec3D::fromYawPitchRad(float yaw, float pitch)
{
	return Vec3D(cos(yaw) * cos(pitch), sin(yaw) * cos(pitch), sin(pitch));
}

Vec3D Vec3D::fromYawPitchDeg(float yaw, float pitch)
{
	yaw *= 0.01745329251994329576923690f;
	pitch *= 0.01745329251994329576923690f;
	return Vec3D(cos(yaw) * cos(pitch), sin(yaw) * cos(pitch), sin(pitch));
}

Vec3D Vec3D::fromHex(unsigned int hexa)
 {
	int b = hexa & 0xFF;
	hexa >>= 8;
	int g = hexa & 0xFF;
	hexa >>= 8;
	int r = hexa & 0xFF;

	return Vec3D(r / 255.0f, g / 255.0f, b / 255.0f);
 }
